/**
* @file imu_icm20602.c in InsCubeBsp
* @author linfe
* @comment  abstract driver for ICM20602
* Create on 2023/1/29 23:33
* @version 1.0
**/

#include <memory.h>
#include "app_config.h"
#include "imu_icm20602.h"

//ACCEL_CONFIG2
#define ICM20602_ACCEL_AVER_4            (0x00<<4)    //Rate=8k
#define ICM20602_ACCEL_AVER_8            (0x01<<4)
#define ICM20602_ACCEL_AVER_16            (0x02<<4)
#define ICM20602_ACCEL_AVER_32            (0x03<<4)

//ACCEL_DLPF
#define ICM20602_ACCEL_DLPF_BW_218            0x00
//#define ICM20602_ACCEL_DLPF_BW_218         	0x01
#define ICM20602_ACCEL_DLPF_BW_99            0x02
#define ICM20602_ACCEL_DLPF_BW_44            0x03
#define ICM20602_ACCEL_DLPF_BW_21            0x04
#define ICM20602_ACCEL_DLPF_BW_10            0x05
#define ICM20602_ACCEL_DLPF_BW_5            0x06
#define ICM20602_ACCEL_DLPF_BW_420           0x06

/*ICM20_INT_ENABLE*/
#define ICM20602_FIFO_OFLOW_EN                (1<<4) //in ICM20_INT_ENABLE reg(0x38)
#define ICM20602_FSYNC_INT_EN                 (1<<3) //in ICM20_INT_ENABLE reg(0x38)
#define ICM20602_GDRIVE_INT_EN                (1<<2) //in ICM20_INT_ENABLE reg(0x38)
#define ICM20602_DATA_RDY_INT_EN              (1<<0) //in ICM20_INT_ENABLE reg(0x38)
#define ICM20602_GYRO_FIFO_EN                 (1<<4) //in ICM20_FIFO_EN reg(0x23)
#define ICM20602_ACCEL_FIFO_EN                (1<<3) //in ICM20_FIFO_EN reg(0x23)
#define ICM20602_FIFO_EN                      (1<<6) //in ICM20_USER_CTRL(0x6A)
#define ICM20602_FIFO_RST                    (1<<2) // in ICM20_USER_CTRL(0x6A)


/*---------------------------------------------------------------------------------*/
//ICM20602_INT_PIN_CFG
#define ICM20602_INT_LEVEL_H                        (1<<7)
#define ICM20602_INT_LEVEL_L                        (0<<7)//0 ?C The logic level for INT/DRDY pin is active high.
#define ICM20602_INT_OPEN_H                        (1<<6)//1 ?C INT/DRDY pin is configured as open drain.
#define ICM20602_INT_OPEN_L                        (0<<6)
#define ICM20602_LATCH_INT_EN_H                (1<<5)//1 ?C INT/DRDY pin level held until interrupt status is cleared.
#define ICM20602_LATCH_INT_EN_L                (0<<5)//0 - more robust
#define ICM20602_INT_RD_CLEAR_H                (1<<4)
#define ICM20602_INT_RD_CLEAR_L                (0<<4)//0 ?C Interrupt status is cleared only by reading INT_STATUS register
#define ICM20602_FSYNC_INT_LEVEL_H            (1<<3)//0 ?C The logic level for the FSYNC pin as an interrupt is active high.
#define ICM20602_FSYNC_INT_LEVEL_L            (0<<3)


//CONFIG DPF
#define ICM20602_DLPF_BW_250            0x00    //Rate=8k
#define ICM20602_DLPF_BW_176            0x01
#define ICM20602_DLPF_BW_92            0x02
#define ICM20602_DLPF_BW_41            0x03
#define ICM20602_DLPF_BW_20            0x04
#define ICM20602_DLPF_BW_10            0x05
#define ICM20602_DLPF_BW_5            0x06
#define ICM20602_DLPF_BW_328            0x06    //Rate=8k

#define ICM20602_ACCEL_FS_2G  (0 << 3)
#define  ICM20602_ACCEL_FS_4G = (1 << 3)
#define  ICM20602_ACCEL_FS_8G = (2 << 3)
#define  ICM20602_ACCEL_FS_16G = (3 << 3)
#define   ICM20602_GYRO_FS_2G  (0 << 3)
#define   ICM20602_GYRO_FS_4G  (1 << 3)
#define   ICM20602_GYRO_FS_8G  (2 << 3)
#define   ICM20602_GYRO_FS_16G  (3 << 3)

typedef enum {

  ICM20602_XG_OFFS_TC_H = 0x04,
  ICM20602_XG_OFFS_TC_L = 0x05,
  ICM20602_YG_OFFS_TC_H = 0x07,
  ICM20602_YG_OFFS_TC_L = 0x08,
  ICM20602_ZG_OFFS_TC_H = 0x0A,
  ICM20602_ZG_OFFS_TC_L = 0x0B,
  ICM20602_SELF_TEST_X_ACCEL = 0x0D,
  ICM20602_SELF_TEST_Y_ACCEL = 0x0E,
  ICM20602_SELF_TEST_Z_ACCEL = 0x0F,
  ICM20602_XG_OFFS_USRH = 0x13,
  ICM20602_XG_OFFS_USRL = 0x14,
  ICM20602_YG_OFFS_USRH = 0x15,
  ICM20602_YG_OFFS_USRL = 0x16,
  ICM20602_ZG_OFFS_USRH = 0x17,
  ICM20602_ZG_OFFS_USRL = 0x18,
  ICM20602_SMPLRT_DIV = 0x19,
  ICM20602_CONFIG = 0x1A,
  ICM20602_GYRO_CONFIG = 0x1B,
  ICM20602_ACCEL_CONFIG = 0x1C,
  ICM20602_ACCEL_CONFIG_2 = 0x1D,
  ICM20602_LP_MODE_CFG = 0x1E,
  ICM20602_ACCEL_WOM_X_THR = 0x20,
  ICM20602_ACCEL_WOM_Y_THR = 0x21,
  ICM20602_ACCEL_WOM_Z_THR = 0x22,
  ICM20602_FIFO_ENABLE = 0x23,
  ICM20602_FSYNC_INT = 0x36,
  ICM20602_INT_PIN_CFG = 0x37,
  ICM20602_INT_ENABLE = 0x38,
  ICM20602_FIFO_WM_INT_STATUS = 0x39,
  ICM20602_INT_STATUS = 0x3A,
  ICM20602_ACCEL_XOUT_H = 0x3B,
  ICM20602_ACCEL_XOUT_L = 0x3C,
  ICM20602_ACCEL_YOUT_H = 0x3D,
  ICM20602_ACCEL_YOUT_L = 0x3E,
  ICM20602_ACCEL_ZOUT_H = 0x3F,
  ICM20602_ACCEL_ZOUT_L = 0x40,
  ICM20602_TEMP_OUT_H = 0x41,
  ICM20602_TEMP_OUT_L = 0x42,
  ICM20602_GYRO_XOUT_H = 0x43,
  ICM20602_GYRO_XOUT_L = 0x44,
  ICM20602_GYRO_YOUT_H = 0x45,
  ICM20602_GYRO_YOUT_L = 0x46,
  ICM20602_GYRO_ZOUT_H = 0x47,
  ICM20602_GYRO_ZOUT_L = 0x48,
  ICM20602_SELF_TEST_X_GYRO = 0x50,
  ICM20602_SELF_TEST_Y_GYRO = 0x51,
  ICM20602_SELF_TEST_Z_GYRO = 0x52,
  ICM20602_FIFO_WM_TH1 = 0x60,
  ICM20602_FIFO_WM_TH2 = 0x61,
  ICM20602_SIGNAL_PATH_RESET = 0x68,
  ICM20602_ACCEL_INTEL_CTRL = 0x69,
  ICM20602_USER_CTRL = 0x6A,
  ICM20602_PWR_MGMT_1 = 0x6B,
  ICM20602_PWR_MGMT_2 = 0x6C,
  ICM20602_I2C_IF = 0x70,
  ICM20602_FIFO_COUNTH = 0x72,
  ICM20602_FIFO_COUNTL = 0x73,
  ICM20602_FIFO_R_W = 0x74,
  ICM20602_WHO_AM_I = 0x75,
  ICM20602_XA_OFFSET_H = 0x77,
  ICM20602_XA_OFFSET_L = 0x78,
  ICM20602_YA_OFFSET_H = 0x7A,
  ICM20602_YA_OFFSET_L = 0x7B,
  ICM20602_ZA_OFFSET_H = 0x7D,
  ICM20602_ZA_OFFSET_L = 0x7E,
} icm20602_reg_addr_t;

typedef enum {
  ICM20602_SPI_W = 0x00,
  ICM20602_SPI_R = 0x80
} icm20602_cmd_t;

icm_attribute_t icm20602_attr = {
    .acce_scale = 0,
    .gyro_scale = 0,
    .max_sample_rate =1,
    .name = "ICM20602",
    .id = 0,
};
icm2060x_handle_t *icm_handle_create() {
    icm2060x_handle_t *handle = bsp_malloc(sizeof(icm2060x_handle_t));
    handle->dev = &spi1_dev;
    handle->attrib = &icm20602_attr;
    return handle;
}

static void icm20602_write_reg(bsp_device_t *spi, icm20602_reg_addr_t addr, uint8_t val) {
    uint8_t dat[2];
    dat[0] = addr | ICM20602_SPI_W;
    dat[1] = val;
    dev_write(spi, dat, 2);
}

static uint8_t icm20602_read_reg(bsp_device_t *spi, icm20602_reg_addr_t addr) {
    uint8_t dat[2];
    dat[0] = addr | ICM20602_SPI_R;
    dat[1] = 0;
    dev_write(spi, dat, 1);
    dev_read(spi, dat, 2);
    return dat[1];
}
#include "main.h"
#include "spi.h"
/**
 *
 * @param spi
 * @param addr
 * @param buffer
 * @param size
 */
#include "app_trace.h"
static icm2060x_error_t icm20602_read_reg_n(bsp_device_t *spi,
                                            icm20602_reg_addr_t addr,
                                            uint8_t *buffer,
                                            size_t size) {
#if 0
    /* TODO  I don't know why this does NOT work !!! */
    uint8_t dat[2];
    dat[0] = ICM20602_FIFO_R_W | ICM20602_SPI_R;
    dat[1] = 0;
    dev_write(spi, dat, 1);
    dev_read(spi, buffer, 14);
    return icm2060x_OK;
#else
    uint8_t dat[2];
    dat[0] = ICM20602_FIFO_R_W | ICM20602_SPI_R;
    dat[1] = 0;
    HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_RESET);
    HAL_SPI_Transmit(&hspi1, dat, 1, 500);
    HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_RESET);
    HAL_SPI_Receive(&hspi1, buffer, size, 500);
    HAL_GPIO_WritePin(SPI1_CS_GPIO_Port, SPI1_CS_Pin, GPIO_PIN_SET);
    return icm2060x_OK;
#endif

}

static icm2060x_error_t icm20602_check_id(icm2060x_handle_t *handle) {
    uint8_t dat, cnt = 0;
    const int mac_cnt = 20;
    do {
        cnt++;
        handle->delay_ms(300);
        dat = icm20602_read_reg(handle->dev, ICM20602_WHO_AM_I);
#if APP_DEBUG_ENABLE == 1
        handle->trace("read imu id:%d\n", dat);
#endif
    } while (dat != 0x12 && cnt < mac_cnt);
    if (cnt >= mac_cnt) {
        handle->trace("icm20602 ID read failed(%d)\n", dat);
        return icm2060x_wrong_id;
    }
    return icm2060x_OK;
}

static void icm2060x_enable_fifo(icm2060x_handle_t *handle) {
    handle->delay_ms(50);
    icm20602_write_reg(handle->dev, ICM20602_FIFO_ENABLE, ICM20602_GYRO_FIFO_EN | ICM20602_ACCEL_FIFO_EN);
    handle->delay_ms(50);
//    icm20602_write_reg(handle->dev, ICM20602_FIFO_WM_TH1, 0x00);
//    handle->delay_ms(50);
//    icm20602_write_reg(handle->dev, ICM20602_FIFO_WM_TH2, 14);
//    handle->delay_ms(50);
    icm20602_write_reg(handle->dev, ICM20602_USER_CTRL, ICM20602_FIFO_EN);
}

static void icm2060x_enable_interrupt(icm2060x_handle_t *handle) {
    handle->delay_ms(50);
    icm20602_write_reg(handle->dev, ICM20602_INT_ENABLE, ICM20602_DATA_RDY_INT_EN);
    handle->delay_ms(50);
    icm20602_write_reg(handle->dev, ICM20602_INT_PIN_CFG,
                       ICM20602_INT_LEVEL_L | ICM20602_INT_OPEN_L | ICM20602_LATCH_INT_EN_L | ICM20602_INT_RD_CLEAR_H);
}

static icm2060x_error_t icm20602_configure(icm2060x_handle_t *handle) {
    /*reset device*/
    uint8_t val, cnt = 0;
    handle->trace("reset IMU...\n");
    icm20602_write_reg(handle->dev, ICM20602_PWR_MGMT_1, 0x80);
    handle->delay_ms(50);
    do {
        val = icm20602_read_reg(handle->dev, ICM20602_PWR_MGMT_1);
        cnt++;
    } while (val != 0x41 && (cnt < 5));
    if (cnt == 5) {
        return icm2060x_reset_failed;
    }
    handle->delay_ms(50);
    /*enable temperature sensor*/
    icm20602_write_reg(handle->dev, ICM20602_PWR_MGMT_1, 0x01);

    handle->delay_ms(50);
    /* set configure sample rate: rate = 1000 / (ICM20602_SMPLRT_DIV + 1)*/
    switch (handle->config.sample_rate) {
        case ICM20602SampleRate_10Hz:icm20602_write_reg(handle->dev, ICM20602_SMPLRT_DIV, 99);
            handle->attrib->delta_t = 1.0f / 10.0f;
            break;
        case ICM20602SampleRate_50Hz:icm20602_write_reg(handle->dev, ICM20602_SMPLRT_DIV, 19);
            handle->attrib->delta_t = 1.0f / 50.0f;
            break;
        case ICM20602SampleRate_100Hz:icm20602_write_reg(handle->dev, ICM20602_SMPLRT_DIV, 9);
            handle->attrib->delta_t = 1.0f / 100.0f;
            break;
        case ICM20602SampleRate_200Hz:icm20602_write_reg(handle->dev, ICM20602_SMPLRT_DIV, 4);
            handle->attrib->delta_t = 1.0f / 200.0f;
            break;
        case ICM20602SampleRate_500Hz:icm20602_write_reg(handle->dev, ICM20602_SMPLRT_DIV, 1);
            handle->attrib->delta_t = 1.0f / 500.0f;
            break;
        case ICM20602SampleRate_1000Hz:icm20602_write_reg(handle->dev, ICM20602_SMPLRT_DIV, 0);
            handle->attrib->delta_t = 1.0f / 1000.0f;
            break;
        default:icm20602_write_reg(handle->dev, ICM20602_SMPLRT_DIV, 0);
            handle->attrib->delta_t = 1.0f / 1000.0f;
    }
    handle->delay_ms(50);
    icm20602_write_reg(handle->dev, ICM20602_CONFIG, ICM20602_DLPF_BW_5);
    /* set full scale */
    switch (handle->config.gyro_full_scale) {
        case ICM20602GyroFs_250DEG:handle->attrib->gyro_scale = 1.0f / 131.068f;
            icm20602_write_reg(handle->dev, ICM20602_GYRO_CONFIG, 0 << 3);
            break;
        case ICM20602GyroFs_500DEG:handle->attrib->gyro_scale = 1.0f / 65.534f;
            icm20602_write_reg(handle->dev, ICM20602_GYRO_CONFIG, 1 << 3);
            break;
        case ICM20602GyroFs_1000DEG:handle->attrib->gyro_scale = 1.0f / 32.767f;
            icm20602_write_reg(handle->dev, ICM20602_GYRO_CONFIG, 2 << 3);
            break;
        case ICM20602GyroFs_2000DEG:handle->attrib->gyro_scale = 1.0f / 16.3835f;
            icm20602_write_reg(handle->dev, ICM20602_GYRO_CONFIG, 3 << 3);
            break;
        default:handle->attrib->gyro_scale = 1.0f / 131.068f;
            icm20602_write_reg(handle->dev, ICM20602_GYRO_CONFIG, 0 << 3);
    }
    handle->delay_ms(50);
    switch (handle->config.acce_full_scale) {
        case ICM20602AcceFs_2G:handle->attrib->acce_scale = 1.0f / 16384.0f;
            icm20602_write_reg(handle->dev, ICM20602_ACCEL_CONFIG, 0 << 3);
            break;
        case ICM20602AcceFs_4G:handle->attrib->acce_scale = 1.0f / 8192.0f;
            icm20602_write_reg(handle->dev, ICM20602_ACCEL_CONFIG, 1 << 3);
            break;
        case ICM20602AcceFs_8G:handle->attrib->acce_scale = 11.0f / 4096.0f;
            icm20602_write_reg(handle->dev, ICM20602_ACCEL_CONFIG, 2 << 3);
            break;
        case ICM20602AcceFs_16G:handle->attrib->acce_scale = 1.0f / 2048.0f;
            icm20602_write_reg(handle->dev, ICM20602_ACCEL_CONFIG, 3 << 3);
            break;
        default:handle->attrib->gyro_scale = 1.0f / 16384.0f;
            icm20602_write_reg(handle->dev, ICM20602_ACCEL_CONFIG, 0 << 3);
    }

    handle->delay_ms(50);
    icm20602_write_reg(handle->dev, ICM20602_ACCEL_CONFIG_2, ICM20602_ACCEL_AVER_4 | ICM20602_ACCEL_DLPF_BW_10);

    /*enable fifo and interrupt */
    icm2060x_enable_fifo(handle);
    icm2060x_enable_interrupt(handle);
    /*open gyroscope and accelerator*/
    handle->delay_ms(50);
    icm20602_write_reg(handle->dev, ICM20602_PWR_MGMT_2, 0x00);

    return icm2060x_OK;
}

icm2060x_error_t icm2060x_init(icm2060x_handle_t *handle) {
    handle->trace("icm20602 init\n");
    /* check ID */
    if (icm20602_check_id(handle)) {
        return icm2060x_wrong_id;
    };
    /* configure */
    if (icm20602_configure(handle)) {
        return icm2060x_reset_failed;
    }
    return icm2060x_OK;
}
#define ALIGN_RAM_D1 __attribute__((section (".ram_d1")))

#define UINT8_2_INT16(high, low)  ((((uint16_t)(high))<< 8)|(low))

icm2060x_error_t icm2060x_read(icm2060x_handle_t *handle, imu_raw_icm_t *icm) {
    uint8_t buffer[14]={0};// = (uint8_t *) &buf.buffer;
    uint8_t dat[2];
    dat[0] = ICM20602_FIFO_R_W | ICM20602_SPI_R;
    dat[1] = 0;

    icm20602_read_reg_n(handle->dev,ICM20602_FIFO_R_W,buffer,14);
//    dev_write(handle->dev, dat, 1);
//    dev_write(handle->dev, dat, 1);
/*    handle->dev->write(dat,1);
    handle->dev->read(buffer,14);*/
//    handle->dev->write(dat,1);
//    dev_read(handle->dev, buffer, 14);

    icm->acce[0] = UINT8_2_INT16(buffer[0], buffer[1]); //((uint16_t)buffer[0]) << 8u) | buffer[1];
    icm->acce[1] = UINT8_2_INT16(buffer[2], buffer[3]); //((uint16_t)buffer[2] << 8u) | buffer[3];
    icm->acce[2] = UINT8_2_INT16(buffer[4], buffer[5]);// ((uint16_t)buffer[4] << 8u) | buffer[5];
    icm->temp = UINT8_2_INT16(buffer[6], buffer[7]);//((uint16_t)buffer[6] << 8u) | buffer[7];
    icm->gyro[0] = UINT8_2_INT16(buffer[8], buffer[9]);// ((uint16_t)buffer[8] << 8u) | buffer[9];
    icm->gyro[1] = UINT8_2_INT16(buffer[10], buffer[11]);//((uint16_t)buffer[10] << 8u) | buffer[11];
    icm->gyro[2] = UINT8_2_INT16(buffer[12], buffer[13]);//((uint16_t)buffer[12] << 8u) | buffer[13];
    return icm2060x_OK;
}
